% Demostrate posterior distribution after fetching distance measurement
% Using Particle Filter Perception
% 2D space 

close all; clc; rng(10);


iterate  = 1;


dSigma     =  0.4;

pointNumber = 5;

dim  = 2;

particleSize = 200;

p = cell(pointNumber,1);

sigmaMean  =  50;



% area length and width 
area.l = 1000;
area.w = 1000;


% initialize

for i = 1: pointNumber
    pTmp.mean  =  rand(2,1).*[area.l;area.w];
    R          =  sigmaMean*rand(2);
    pTmp.cov   =  R*R';
    pTmp.cov(1,2) = pTmp.cov(1,2) /10;
    pTmp.cov(2,1) =pTmp.cov(2,1) ;
    p{i} =  pTmp;
end

particles = zeros(pointNumber*dim,particleSize);

% every dim * row represents an agent
for i = 1:pointNumber
    R = chol(p{i}.cov);
    z = repmat(p{i}.mean,1,particleSize) + R*randn(2,particleSize);
    particles((2*i-1):(2*i),:) = z;
end

figure(1)
axis equal
hold on

for i = 1:pointNumber
    plot(particles(2*i-1,:),particles(2*i,:),'+')
end

% Measurement generating
d  = zeros(pointNumber);

for i =1: pointNumber
    for j  = 1: i-1
        p1True = p{i}.mean;
        p2True = p{j}.mean;
        d(i,j) = norm(p1True-p2True) + randn(1)*dSigma;
        d(j,i) = d(i,j);
    end
end

% compute posterior distribution 

for iterateIndex  = 1:iterate
    for i = 1: pointNumber
        for mIndex  = 1:i-1
            % resampling 
            disp(mIndex)
            qs = zeros(particleSize,1);
            for j  =  1: particleSize
                for k = 1:particleSize
                    p1 = particles(2*i-1:2*i,j);
                    p2 = particles(2*mIndex-1:2*mIndex,k);
                    qs(j) = qs(j)+ likelihood(p1,p2,d(i,mIndex),dSigma);
                end
            end

            qs =  qs  /sum(qs);
            ind  = resampleSystematic(qs,particleSize);
            particles((2*i-1):(2*i),:) = particles((2*i-1):(2*i),ind);

        end
    end
end

figure(2)
axis equal
hold on

for i = 1:pointNumber
    plot(particles(2*i-1,:),particles(2*i,:),'+')
end




